#!/usr/bin/env python

import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
#from visualization_msgs.msg import Marker
from math import radians, pi
from std_msgs.msg import String
class MoveBaseSquare():
    def __init__(self):
        rospy.init_node('location_move', anonymous=False)
        rospy.on_shutdown(self.shutdown)

        self.serial_read = rospy.Subscriber('read',String,self.callback)
        self.serial_read = rospy.Subscriber("read1",String,self.callback1)
        self.serial_read = rospy.Subscriber("read2",String,self.callback2)#

	self.cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=5)
        self.read_pub = rospy.Publisher('write',String, queue_size=1)
     
    def callback(self, data):         
    	rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.data)
    	if(data.data=="a"):               #8
        	self.read_pub.publish("Please input x")

    def callback1(self, data):      #111   
    	rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.data)
        global x
    	x=float(data.data)
        self.read_pub.publish("Please input y")

    def callback2(self, data):         
    	rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.data)
        global y
    	y=float(data.data)
        self.read_pub.publish("OK!")               
	quaternions = list()
	euler_angles = (pi/2, pi, 3*pi/2, 0)
	for angle in euler_angles:
		q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
		q = Quaternion(*q_angle)
		quaternions.append(q)

	waypoints = list()
	waypoints.append(Pose(Point(x,y,0.0), quaternions[3]))

	self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
	rospy.loginfo("Waiting for move_base action server...")
	self.move_base.wait_for_server(rospy.Duration(60))
	rospy.loginfo("Connected to move base server")
	rospy.loginfo("Starting navigation test")
	i=0
	while i<1 and not rospy.is_shutdown():
		goal = MoveBaseGoal()
		goal.target_pose.header.frame_id = 'map'
		goal.target_pose.header.stamp = rospy.Time.now()
		goal.target_pose.pose = waypoints[0]
		self.move(goal)
		i += 1

    def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
        self.move_base.send_goal(goal)
            
            # Allow 2 minute to get there
        finished_within_time = self.move_base.wait_for_result(rospy.Duration(120)) 
            
            # If we don't get there in time, abort the goal
        if not finished_within_time:
            self.move_base.cancel_goal()
            rospy.loginfo("Timed out achieving goal")
            self.read_pub.publish("Timed out achieving goal");
        else:
                # We made it!
            state = self.move_base.get_state()
            if state == GoalStatus.SUCCEEDED:
               rospy.loginfo("Goal succeeded!")
               self.read_pub.publish("Goal succeeded!");
                    
    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Cancel any active goals
        self.move_base.cancel_goal()
        rospy.sleep(2)
        # Stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        MoveBaseSquare()
	rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")
